//VERSION 1.0 - Flown on Mini Bexus 26. Sept 2012 to ~10km alt
//added HI

// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-

// FIXME cleanup: make homebase_dist a global variable thats (only) calculated once per cycle (post-launch)

#include <FastSerial.h>
#include <SPI.h>
#include <I2C.h>
#include <Arduino_Mega_ISR_Registry.h>
#include <AP_PeriodicProcess.h>
#include <AP_InertialSensor.h>
#include <AP_ADC.h>
#include <AP_IMU.h>
#include <AP_GPS.h>
#include <AP_AHRS.h>
#include <AP_Math.h>
#include <AP_Common.h>
#include <AP_Compass.h>
#include <AP_Baro.h>
#include <DataFlash.h>
#include <APM_RC.h>
//#include <GCS_MAVLink.h>
#include <Filter.h>
#include "vx_var.h"




FastSerialPort(Serial, 0);
FastSerialPort(Serial1, 1);
FastSerialPort(Serial3, 3);

Arduino_Mega_ISR_Registry isr_registry;
AP_TimerProcess  scheduler;


AP_Compass_HMC5843 compass;

//AP_Baro_BMP085 barometer(0); FIXME Baro? (post-launch)

AP_ADC_ADS7844          adc;
AP_InertialSensor_Oilpan ins( &adc );


static GPS *g_gps;
AP_GPS_UBLOX g_gps_driver(&Serial1);


AP_IMU_INS imu(&ins);

AP_AHRS_Quaternion  ahrs(&imu, g_gps);


# define A_LED_PIN        37
# define C_LED_PIN        35
# define LED_ON           HIGH
# define LED_OFF          LOW
# define MAG_ORIENTATION  AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD



static void flash_leds(bool on)
{
  digitalWrite(A_LED_PIN, on?LED_OFF:LED_ON);
  digitalWrite(C_LED_PIN, on?LED_ON:LED_OFF);
}

//###################### RC initialize begin ###########################
APM_RC_APM1 APM_RC;
//###################### RC initialize end   ###########################


void setup(void)
{

  Serial.begin(115200); // USB
  Serial1.begin(38400); // GPS
  Serial3.begin(57600); // XBEE

  Serial.println("$WRESTARTED");
  Serial3.println("$WRESTARTED");

  //###################### PARACHUTE setup begin ###########################
  vx_para_setup();
  //###################### PARACHUTE setup end ###########################


  //###################### AHRS setup begin ###########################
  SPI.begin();
  SPI.setClockDivider(SPI_CLOCK_DIV16);

  isr_registry.init();
  scheduler.init(&isr_registry);

  imu.init(IMU::COLD_START, delay, flash_leds, &scheduler);
  imu.init_accel(delay, flash_leds);

  compass.set_orientation(MAG_ORIENTATION);
  if (compass.init()) {
    Serial.printf("Enabling compass\n");
    compass.null_offsets_enable();
    ahrs.set_compass(&compass);
  }

  //###################### AHRS setup end ###########################



  //###################### RC setup begin ###########################
  APM_RC.Init(&isr_registry);	 // APM Radio initialization // isr_reg already intitialized @ahrs setup
  APM_RC.enable_out(CH_1);
  APM_RC.enable_out(CH_2);
  APM_RC.enable_out(CH_3);
  APM_RC.enable_out(CH_4);
  APM_RC.enable_out(CH_5);
  APM_RC.enable_out(CH_6);
  APM_RC.enable_out(CH_7);
  APM_RC.enable_out(CH_8);



  //###################### RC setup end ###########################


  // #################### GPS setup begin ##########################
  g_gps = &g_gps_driver;

  stderr = stdout;
  g_gps->print_errors = true;

  g_gps->init();
  //###################### GPS setup end ###########################

  // #################### Baro setup begin ##########################
  //if (!barometer.init(&scheduler)) {
  //Serial.println("Barometer initialisation FAILED\n");               //FIXME Baro? (post-launch)
  //}
  // #################### Baro setup end ##########################



};




void loop(void)
{



  //###################### AHRS Loop begin ###########################  

  static uint16_t counter;
  static uint32_t last_t, last_print, last_compass;
  uint32_t now = micros();

  if (last_t == 0) {
    last_t = now;
    return;
  }
  last_t = now;

  if (now - last_compass > 100*1000UL &&
    compass.read()) {
    compass.calculate(ahrs.get_dcm_matrix());
    // read compass at 10Hz
    last_compass = now;
  }

  ahrs.update();

  #if VX_SIM == VX_SIM_HIL  
    vx_HILCheckMessages();    
  #endif




    counter++;

  if (now - last_print >= 0.5e6) {
    Vector3f drift  = ahrs.get_gyro_drift();
    /*Serial.printf_P(PSTR("r:%4.1f  p:%4.1f y:%4.1f drift=(%5.1f %5.1f %5.1f) hdg=%.1f rate=%.1f\n"),
     						ToDeg(ahrs.roll),
     						ToDeg(ahrs.pitch),
     						ToDeg(ahrs.yaw),
     						ToDeg(drift.x),
     						ToDeg(drift.y),
     						ToDeg(drift.z),
     						compass.use_for_yaw()?ToDeg(compass.heading):0.0,
     						(1.0e6*counter)/(now-last_print));
     */
    last_print = now;
    counter = 0;
  };
  //###################### AHRS Loop end ###########################

  //###################### VX Loop begin ###########################

  // Fast loop
  vx_stabilize();
  vx_svout();

  // Medium loop
  switch(vx_g_loop_medium_i) {
  case 0: // Flightplan
    vx_pos_update();
    vx_flightplan();
    break;
  case 1: // Sequencer
    vx_sequencer();
    break;
  case 2: // Incoming telecommands
    vx_teleCheckTelecommands();
  default:
    break;
  }
  vx_g_loop_medium_i++;
  if(vx_g_loop_medium_i > 2) {
    vx_g_loop_medium_i = 0;
  }


  //###################### VX Loop end ###########################    




  // New radio frame? (we could use also if((millis()- timer) > 20)

  //only used when switch not manual!!! laggy performance !!

  // if (APM_RC.GetState() == 1){
  // 	Serial.print("CH:");
  // 	for(int i = 0; i < 8; i++){
  // 		Serial.print(APM_RC.InputCh(i));	// Print channel values
  // 		Serial.print(",");
  // 		APM_RC.OutputCh(i, APM_RC.InputCh(i)); // Copy input to Servos
  // 	}
  // 	Serial.println();
  // }



  //only on first loop. kind of second setup routine, but with every component active
  /*if(vx_firstloop)
   {
   //calibration of stick center
   vx_g_setmiddledeltawing();
   
   //calculate difference between middle and full stick position to calibrate actual stick travel for percentual stick travel
   vx_g_setdiffdeltawing(); 
   
   vx_firstloop = false;
   }
   */




};

void vx_sequencer() {
  // Update the timer
  //if(vx_g_act_settime) {
  //  vx_g_act_time = millis();
  //  vx_g_act_settime = false;
  //}

  // Increment act_case every 100 ms
  if((millis()-vx_g_act_time) > 100) {
    vx_g_act_case++;
    vx_g_act_case_updated = true;
    vx_g_act_time = millis(); // Update timer


  }

  // Reset act_case when exceeding 100 (actually: 100*100ms = 10 seconds [10000ms])
  if(vx_g_act_case > 99) {
    vx_g_act_case = 0;
    //Serial.println("DEBUG: vx_sequencer(): reseting act_case");
    //Serial.print("DEBUG: vx_sequencer(): incrementing: act_case = ");
    //Serial.print(vx_g_act_case);
    //Serial.print("DEBUG: vx_sequencer(): act_time = ");
    //Serial.println(vx_g_act_time);
  }

  if(vx_g_act_case_updated) {
    // The actual sequencer

    // vx_g_calculate_navdampening = true; CHANGE

    // Parachute release command
    vx_para_loop();


    /*
    Serial.print("Last Auto Mode: ");
     Serial.println(vx_g_last_autonomous_mode);
     
     
     Serial.print("AHRS YAW: "); // FIXME debug (post-launch)
     Serial.print(ToDeg(ahrs.yaw));
     Serial.print(" GPS Hd: ");
     Serial.print(g_gps->ground_course/100.);
     Serial.print(" Target Hd: ");
     Serial.print(ToDeg(vx_g_target_heading));
     Serial.print(" Target Diff: ");
     Serial.print(ToDeg(vx_error_heading));
     Serial.print(" Target Roll: ");
     Serial.print(ToDeg(vx_g_target_roll));
     Serial.print(" Ground Distance: ");
     Serial.println(vx_g_wpdistance);
     
     Serial.print(vx_errorsum_nickrate);
     Serial.println(" ");
     Serial.println(vx_errorsum_rollrate);
     
     
     Serial.print(vx_dalt);
     Serial.print(" ");
     Serial.print(vx_g_wpdistance);
     Serial.print(" ");
     Serial.print(vx_target_flightpathangle);
     Serial.print(" ");
     Serial.print(vx_g_target_pitch);
     Serial.print(" ");
     Serial.print(ToDeg(vx_target_flightpathangle));
     Serial.print(" ");
     Serial.print(ToDeg(vx_g_target_pitch));
     Serial.println(" ");
     */


    // SEQUENCER
    // -----------------------------------------------------------------------
    // TM Position -- T: 3s (cases: 20, 60, 80)
    // TM Attitude -- T: ~3s (cases: 0, 35, 75)
    // TM Housekeeping -- T: 5s (cases: 15, 65)
    //
    // Parchute check -- T: 2s (cases: 13, 33, 53, 73, 93)
    // Launch check -- T: 5s (cases: 45, 95)
    switch(vx_g_act_case) {
    case 0:
      vx_teleSendCurrentAttitude();
      break;

    case 13:
      if(vx_g_pos_had_gps_fix == true) vx_para_check();
      break;
    case 15:
      vx_teleSendHousekeeping();
      break;
    case 20:
      vx_teleSendCurrentPosition();
      break;

    case 33:
      if(vx_g_pos_had_gps_fix == true) vx_para_check();
      break;
    case 35:
      vx_teleSendCurrentAttitude();
      break;


    case 45:
      vx_launch_check();
      break;

    case 53:
      if(vx_g_pos_had_gps_fix == true) vx_para_check();
      break; 

    case 60:
      vx_teleSendCurrentPosition();
      break;
    case 65:
      vx_teleSendHousekeeping();
      break;

    case 73:
      if(vx_g_pos_had_gps_fix == true) vx_para_check();
      break;
    case 75:
      vx_teleSendCurrentAttitude();
      break;

    case 80:
      vx_teleSendCurrentPosition();
      break;

    case 93:
      if(vx_g_pos_had_gps_fix == true) vx_para_check();
      break;
    case 95:
      vx_launch_check();
      break;

    default:
      break;

      /*switch(vx_g_act_case) {
       case 0:
       vx_teleSendCurrentPosition();
       break;
       case 1:
       vx_teleSendCurrentAttitude();
       break;
       case 5:
       vx_teleSendDescent();
       break;
       
       case 7:
       if(vx_g_pos_had_gps_fix == true) vx_para_check();
       break;
       
       case 10:
       vx_teleSendCurrentPosition();
       break;
       case 11:
       vx_teleSendCurrentAttitude();        
       break;
       case 20:
       vx_teleSendHousekeeping();
       break;
       case 25:
       vx_launch_check();
       break;
       
       case 27:
       if(vx_g_pos_had_gps_fix == true) vx_para_check();
       break;
       
       case 30:
       vx_teleSendCurrentAttitude();
       break;
       case 40:
       vx_teleSendCurrentPosition();
       break;
       default:
       break;*/
    }
  }
  vx_g_act_case_updated = false;
}

